// stl相关库
#include <iostream>
#include <vector>
#include <string>
#include <mutex>

// Ros相关库
#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <acupoint_msgs/AcupointImagePixel.h>
#include <acupoint_msgs/AcupointCoordinate.h>


#define FILTER_KERNEL 5

struct Point3D{
    float x;
    float y;
    float z;
    Point3D(){
        x=0;
        y=0;
        z=0;
    }
};

std::mutex m;
ros::Publisher filter_pub;
acupoint_msgs::AcupointCoordinate acu_coordinate_filter_msg;

std::vector<std::vector<Point3D>> origin_data(FILTER_KERNEL);
int flag=-1;


// 均值穴位坐标
void pixelCallback(const acupoint_msgs::AcupointCoordinateConstPtr &msg){

    if(msg->name.size()!=acu_coordinate_filter_msg.name.size()){
        acu_coordinate_filter_msg.name.resize(msg->name.size());
        acu_coordinate_filter_msg.acupoints.resize(msg->name.size());
        for(int i=0;i<FILTER_KERNEL;++i){
            origin_data[i].resize(msg->name.size(),Point3D());
        }
        for(int i=0;i<msg->name.size();++i) acu_coordinate_filter_msg.name[i]=msg->name[i];
        flag=-1;
    }

    if(flag==-1){
        for(int i=0;i<FILTER_KERNEL;++i){
            for(int j=0;j<acu_coordinate_filter_msg.name.size();++j){
                origin_data[i][j].x=msg->acupoints[j].x;
                origin_data[i][j].y=msg->acupoints[j].y;
                origin_data[i][j].z=msg->acupoints[j].z;
            }
        }
        flag=1;
    }
    else{
        for(int j=0;j<acu_coordinate_filter_msg.name.size();++j){
            origin_data[flag][j].x=msg->acupoints[j].x;
            origin_data[flag][j].y=msg->acupoints[j].y;
            origin_data[flag][j].z=msg->acupoints[j].z;
        }
        flag=(flag+1)%FILTER_KERNEL;
    }

    std::vector<Point3D> temp(msg->name.size(),Point3D());
    for(int i=0;i<FILTER_KERNEL;++i){
        for(int j=0;j<acu_coordinate_filter_msg.name.size();++j){
            temp[j].x+=origin_data[i][j].x;
            temp[j].y+=origin_data[i][j].y;
            temp[j].z+=origin_data[i][j].z;
        }
    }

    for(int j=0;j<acu_coordinate_filter_msg.name.size();++j){
        temp[j].x/=FILTER_KERNEL;
        temp[j].y/=FILTER_KERNEL;
        temp[j].z/=FILTER_KERNEL;
        acu_coordinate_filter_msg.acupoints[j].x=temp[j].x;
        acu_coordinate_filter_msg.acupoints[j].y=temp[j].y;
        acu_coordinate_filter_msg.acupoints[j].z=temp[j].z;
    }

    // 发布过滤后的穴位坐标
    acu_coordinate_filter_msg.header.stamp=ros::Time::now();
    filter_pub.publish(acu_coordinate_filter_msg);
}


int main(int argc, char *argv[])
{
    ros::init(argc, argv, "acupoint_coordinate_filter_node");
    ros::NodeHandle nh;  
    filter_pub=nh.advertise<acupoint_msgs::AcupointCoordinate>("/acupoint/acu2base_filter_coordinate",1);
    ros::Subscriber pixel_sub=nh.subscribe<acupoint_msgs::AcupointCoordinate>("/acupoint/acu2base_coordinate",1,pixelCallback);
    ros::spin();
    return 0;
}
